#include "ShooterAngle.h"
#include "../Robotmap.h"
#include "SmartDashboard/SmartDashboard.h"
#include "../Commands/ShooterAngleControl.h"

ShooterAngle::ShooterAngle() :
	PIDSubsystem("ShooterAngle", Kp, Ki, Kd),
			shooterAnglePot(SHOOTER_ANGLE_POT),
			shooterAngleVictor(SHOOTER_ANGLE_VICTOR) {
	// Use these to get going:
	// SetSetpoint() -  Sets where the PID controller should move the system
	//                  to
	// Enable() - Enables the PID controller.
	this->SetInputRange(2, 39);
	this->SetAbsoluteTolerance(1);
	started = false;
	this->SetSetpoint(ShooterHome);
	SetPID(0.175,0.0,0.01);

	/************************************
	 POT POSITIONS
	 ----------------------------------
	 PRACTICE BOT::
	 Lowest:		2.25V	3.5deg	(Measured)
	 2nd Lowest: 2.14V	7.4deg	(Measured)
	 Highest: 	1.125V	39deg	(Measured)
	 .0317 V/deg
	 P: .175
	 I: .000
	 D: .010
	 ************************************
	 COMPETITION BOT::
	 Lowest:
	 2nd Lowest:
	 Highest:
	 v/deg
	 P:
	 I:
	 D:
	 ************************************/
}

float ShooterAngle::ConvertToAngle(float voltage) {
	float angle = (minAngle + (voltage - minVoltage) * (maxAngle - minAngle) / (maxVoltage - minVoltage));
	DSPrintLine(LINE4, "V:%.2f A%.2f:", voltage, angle);
	return angle;
}

float ShooterAngle::GetCurrentShooterAngle() {
	return ConvertToAngle(shooterAnglePot.GetVoltage());
}
double ShooterAngle::ReturnPIDInput() {
	// Return your input value for the PID loop
	// e.g. a sensor, like a potentiometer:
	// yourPot->SetAverageVoltage() / kYourMaxVoltage;
	DSPrintLine(LINE2, "SA: %.2f", GetCurrentShooterAngle());
	return GetCurrentShooterAngle();

}

void ShooterAngle::SetRelative(float a)
{
	SetSetpointRelative(a);
	Enable();
}

void ShooterAngle::SetPID(float P, float I, float D)
{
//	this->GetPIDController()->SetPID(P, I, D);
	this->GetPIDController()->SetPID(
			0.2,
			0,
			0.035
			);
//	this->GetPIDController()->SetPID(CommandBase::client->GetP(), CommandBase::client->GetI(), CommandBase::client->GetD());		
}
void ShooterAngle::UsePIDOutput(double output) {
	if (GetSetpoint() < 3) {
		SetSetpoint(3);
	}
	if (GetSetpoint() > 38.2) {
		SetSetpoint(38.2);
	}
	
	if (!started) {
		started = true;
		SetSetpoint(GetCurrentShooterAngle());
	}
	// Use output to drive your system, like a motor
	// e.g. yourMotor->Set(output);
	
	if(fabs(output) <= .25){
			output = 0.0;
		}
	// TODO: REMOVEEEE
//	output = CommandBase::oi->shooterJoystick.GetRawAxis(2);
//	
	DSPrintLine(LINE3, "PID O: %.4f", output);
	shooterAngleVictor.Set(output);
}

void ShooterAngle::InitDefaultCommand() {
	// Set the default command for a subsystem here.
	//setDefaultCommand(new MySpecialCommand());
//SetDefaultCommand(new ShooterAngleControl(ShooterHome));
	SetDefaultCommand(new ShooterAngleControl(ShooterHome));
}
